//initialisatie
void sendstartschot();
int receiveIDs();
void setIDs(int maxID);
void givedistance(int id);
int progressIDs(int inimessages);
int IDtoekenen(int maxid);
int extractdistance(int nrmessages, int driven, int pogingnr);
int afstandeninlezen();
void initdrive(unsigned char id);
void afstandmeten(int id);
void resend(unsigned char id);

//main loop
void drive(unsigned char id);
int inlezen(int length);
void send(int length);
void sendaarde();
int check();

//extra
void encode(unsigned char number, int bitarray1[8], int bitarray2[8]);
int decode(unsigned char * number, int * bitarray1, int * bitarray2);

//globale variabelen
unsigned char codeddata[64]; //data die we binnenkrijgen
unsigned char data[20]; //data die we binnenkrijgen die gedecodeerd is
unsigned char newdata[32]; //data die we gaan versturen
unsigned char codedsend[64];//te versturen data met hamming
unsigned char afstanden[10];//afstanden voor aarde
unsigned char IDs[10][8];// lange id's 
unsigned char afstandmssg[10][12]; //ontvangen afstand berichten met hamming
unsigned char decodeafstand[10][6];//ontvangen afstand berichten zonder hamming
unsigned char aardevector[10]; //de afstandsvector voor de aarde
unsigned char fullids[100][24]; //lijst met berichten waar de lange id's in zitten
unsigned char allids[100][8]; //dezelfe lijst maar dan zonder dle's start,stop en hamming
int maxID = 0;
char *rs232_port = (char*)0xC00000;//set memory

void main()
{
    
    int a;
    int b;
    int ID = 0;
    int tjek = 0;
    unsigned char c = 0;
    int drivecount = 0;
    int inimessages = 0;
	int leesfout=0;
	unsigned char i=0;
	int messages=0;
	int poging=0;
	
    sendstartschot();
    inimessages = receiveIDs();
    maxID = progressIDs(inimessages);
    maxID = IDtoekenen(maxID);
    for(a = 0; a<maxID; a++)
    {
        initdrive(a);
    }
    for(b = 0; b<maxID; b++)
    {
        afstandmeten(b);
        messages = afstandeninlezen();
		while(1)
		{
			poging = extractdistance(messages, ID, poging);
			if(poging == 0)
			{
				sendaarde();
				if(ID == maxID)
				{
				    ID = 0;
				}
				else
				{
				    ID++;
				}
				break;
		    }
			else if(poging == -1)
			{   
				if(ID == maxID)
				{
					ID = 0;
				}
				else
				{
				    ID++;
				}
				poging = 0;
				break;
			}
			else
			{
				for(i=0;i<maxID;i++)
				{
					resend(i);
					leesfout=inlezen(6);
					aardevector[data[3]]=data[4];
				}
				
			}
		}
	}
    while(1)
    {
        for(a = 0; a<10;a++)
        {
            aardevector[a] = 0;
        }
        drive(ID);
		messages = afstandeninlezen();
		while(1)
		{
		    poging = extractdistance(messages, ID, poging);
		    if(poging == 0)
		    {
		        sendaarde();
		        if(ID == maxID)
		        {
		            ID = 0;
		        }
		        else
		        {
		            ID++;
		        }
				break;
		    }
			else if(poging == -1)
			{   
				if(ID == maxID)
		        {
		            ID = 0;
		        }
		        else
		        {
		            ID++;
		        }
				poging = 0;
				break;
			}
			else
			{
				for(i=0;i<maxID;i++)
				{
					resend(i);
					leesfout=inlezen(6);
					aardevector[data[3]]=data[4];
				}

			}
		}
	}
}

void char2bin(unsigned char number, int bitarray[8]) 
{
    int j;
    for (j=0; j<8; j++) 
	{
        bitarray[j] = ((number >> j) && 1);
    }
}

unsigned char bin2char(int bitarray[8]) 
{
    unsigned char number = 0;
    int i;

    for(i=0; i<8; i++) 
	{
        if(bitarray[i]!=0) 
		{
            number += 1<<i;
        }
    }
    return number;
}

void encode(unsigned char number, int bitarray1[8], int bitarray2[8]) {
    //b3, b3^b2^b1, b2, !b2^b1^b0, b1, !b3^b1^b0, b0, !b3^b2^b0
    int bitarray[8];
    char2bin(number, bitarray);

    bitarray1[7] = (bitarray[3]);
    bitarray1[6] = ((bitarray[3])^(bitarray[2])^(bitarray[1]))==1?1:0;
    bitarray1[5] = (bitarray[2]);
    bitarray1[4] = ((!bitarray[2])^(bitarray[1])^(bitarray[0]))==1?1:0;
    bitarray1[3] = (bitarray[1]);
    bitarray1[2] = ((!bitarray[3])^(bitarray[1])^(bitarray[0]))==1?1:0;
    bitarray1[1] = (bitarray[0]);
    bitarray1[0] = ((!bitarray[3])^(bitarray[2])^(bitarray[0]))==1?1:0;

    bitarray2[7] = (bitarray[7]);
    bitarray2[6] = ((bitarray[7])^(bitarray[6])^(bitarray[5]))==1?1:0;
    bitarray2[5] = (bitarray[6]);
    bitarray2[4] = ((!bitarray[6])^(bitarray[5])^(bitarray[4]))==1?1:0;
    bitarray2[3] = (bitarray[5]);
    bitarray2[2] = ((!bitarray[7])^(bitarray[5])^(bitarray[4]))==1?1:0;
    bitarray2[1] = (bitarray[4]);
    bitarray2[0] = ((!bitarray[7])^(bitarray[6])^(bitarray[4]))==1?1:0;
}

int decode(unsigned char * number, int * bitarray1, int * bitarray2) {
    //    p  = h7 ^ h6 ^ h5 ^ h4 ^ h3 ^ h2 ^ h1 ^ h0
    //    c0 = h7 ^ h5 ^ h1 ^ h0
    //    c1 = h7 ^ h3 ^ h2 ^ h1
    //    c2 = h5 ^ h4 ^ h3 ^ h1
    int bitarray[8];
    int p1, p2, c01, c02, c11, c12, c21, c22;
    
    p1  = ((bitarray1[7]) ^ (bitarray1[6]) ^ (bitarray1[5]) ^ (bitarray1[4]) ^ (bitarray1[3]) ^ (bitarray1[2]) ^ (bitarray1[1]) ^ (bitarray1[0]))==1?1:0;
    c01 = ((bitarray1[7]) ^ (bitarray1[5]) ^ (bitarray1[1]) ^ (bitarray1[0]))==1?1:0;
    c11 = ((bitarray1[7]) ^ (bitarray1[3]) ^ (bitarray1[2]) ^ (bitarray1[1]))==1?1:0;
    c21 = ((bitarray1[5]) ^ (bitarray1[4]) ^ (bitarray1[3]) ^ (bitarray1[1]))==1?1:0;

    p2  = ((bitarray2[7]) ^ (bitarray2[6]) ^ (bitarray2[5]) ^ (bitarray2[4]) ^ (bitarray2[3]) ^ (bitarray2[2]) ^ (bitarray2[1]) ^ (bitarray2[0]))==1?1:0;
    c02 = ((bitarray2[7]) ^ (bitarray2[5]) ^ (bitarray2[1]) ^ (bitarray2[0]))==1?1:0;
    c12 = ((bitarray2[7]) ^ (bitarray2[3]) ^ (bitarray2[2]) ^ (bitarray2[1]))==1?1:0;
    c22 = ((bitarray2[5]) ^ (bitarray2[4]) ^ (bitarray2[3]) ^ (bitarray2[1]))==1?1:0;

    // If the parity, p, is correct (equal to 1) then either 0 or 2 errors occurred. If all the check bits, c0, c1, c2 are correct (equal to 1) then the byte was received intact, (no errors) otherwise it was damaged beyond repair (two errors).
    if((p1 && !(c01 && c11 && c21)) || (p2 && !(c02 && c12 && c22))) {
        return 0;
    }

    // h7, h5, h3, h1
    bitarray[7] = (bitarray2[7]);
    bitarray[6] = (bitarray2[5]);
    bitarray[5] = (bitarray2[3]);
    bitarray[4] = (bitarray2[1]);
    bitarray[3] = (bitarray1[7]);
    bitarray[2] = (bitarray1[5]);
    bitarray[1] = (bitarray1[3]);
    bitarray[0] = (bitarray1[1]);

    //     c0 c1 c2    meaning
    //     0  0  1    error in bit h7
    //     0  1  0    error in bit h5
    //     1  0  0    error in bit h3
    //     0  0  0    error in bit h1
    if(!p2) {
        if(!c02 && !c12 && c22)
            bitarray[7] = !(bitarray[7]);
        if(!c02 && c12 && !c22)
            bitarray[6] = !(bitarray[6]);
        if(c02 && !c12 && !c22)
            bitarray[5] = !(bitarray[5]);
        if(!c02 && !c12 && !c22)
            bitarray[4] = !(bitarray[4]);
    }
    if(!p1) {
        if(!c01 && !c11 && c21)
            bitarray[3] = !(bitarray[3]);
        if(!c01 && c11 && !c21)
            bitarray[2] = !(bitarray[2]);
        if(c01 && !c11 && !c21)
            bitarray[1] = !(bitarray[1]);
        if(!c01 && !c11 && !c21)
            bitarray[0] = !(bitarray[0]);
    }

    *number = bin2char(bitarray);
    return 1;
}

int check(int maxid)
{
    char errors = 0;
    char i = 0;
    for(i = 0; i < maxid; i++)
    {
        if(data[i] < 20)
        {
            data[i] = 255;
            errors = errors + 1;
        }
        else if(data[i] > 200)
        {
            data[i] = 255;
            errors = errors +1;
        }
    }
    if(errors > maxid-3)
    {
        return 0;
    }
    else
    {
        return 1;
    }
}

void sendstartschot()
{
	newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 33;// opcode 33 voor initialisatie
	newdata[3] = 16;//dle
	newdata[4] = 20;//stop bug
    newdata[5] = '\r';
    newdata[6] = '\n';
	send(7);
}

void afstandmeten(int id)
{
	newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 77;//opcode 77 voor alleen afstanden meten naar karretje <id>
	newdata[3] = id;//welk autootje 
	newdata[4] = 16;//dle
    newdata[5] = 20;//stop bug
    newdata[6] = '\r';
	newdata[7] = '\n';
	send(7);
}

void initdrive(unsigned char id)
{
    newdata[0]=16;//dle
	newdata[1]=19;//start bug
	newdata[2]=66;//opcode voor rijden
	newdata[3]=id;//id auto
	newdata[4]=16;//dle
	newdata[5]=20;//stop bug
	newdata[6]='\r';
	newdata[7]='\n';
	send(8);
}

void drive(unsigned char id)
{
    newdata[0]=16;//dle
	newdata[1]=19;//start bug
	newdata[2]=11;//opcode voor rijden
	newdata[3]=id;//id auto
	newdata[4]=16;//dle
	newdata[5]=20;//stop bug
	newdata[6]='\r';
	newdata[7]='\n';
	send(8);
}

void resend(unsigned char id)
{
    newdata[0]=16;//dle
	newdata[1]=19;//start bug
	newdata[2]=66;//opcode voor opnieuw afstand sturen.
	newdata[3]=id;//id auto
	newdata[4]=16;//dle
	newdata[5]=20;//stop bug
	newdata[6]='\r';
	newdata[7]='\n';
	send(8);
}

void givedistance(int id)
{
    newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 55;// opcode 55 voor afstand
    newdata[3] = id;// id tot welk autootje afstand moet worden gemeten
	newdata[4] = 16;//dle
	newdata[5] = 20;//stop bug
    newdata[6] = '\r';
    newdata[7] = '\n';
	send(8);
}

void send(int length)     
{
	int mssgnr=0;
	int codenr=0;
	int skip=0;
	int dle=0;
	int bit1[8];
	int bit2[8];
	int i=0;
	int lengte=0;
	while(mssgnr<length)
	{		
		skip=codenr*2;
		encode(data[mssgnr], bit1, bit2);
		codedsend[skip] = bin2char(bit1);
		codedsend[skip+1] = bin2char(bit2);
		if(newdata[mssgnr]==16 && dle==0)
		{
			dle=1;
			i--;
		}
		else if(dle==1)
		{
			dle=0;
		}
		mssgnr++;
		codenr++;
	}
	lengte=codenr*2;
	while(i<lengte)
	{
		rs232_port[0] = codedsend[i];
	}
}

int IDtoekenen(int maxid) 
{
	int i=0;
	int j=0;
	int b=0;
	int carnumber=0;
	int skip=0;
	unsigned char input=0;
	newdata[0] = 16;//dle
	newdata[1] = 19;//start bug
    newdata[2] = 44;//opcode 44 voor toekenen id
	//lange id en nieuwe id
	newdata[12] = 16;//dle
	newdata[13] = 20;//stop bug
    newdata[14] = '\r';
    newdata[15] = '\n';
	for(i=0;i<maxid;i++)
	{
		for(j=0;j<8;j++)
		{
			newdata[j+3]=IDs[i][j];
		}
		newdata[11]=carnumber;
		send(16);
		b=0;
		while(1)
		{
			input = rs232_port[0];

			if(input == 100)
			{
				carnumber++;
				break;
			}
			else if(input == 69)
			{
				b++;
			}
			if(b == 5)
			{
				break;
			}	
		}
	}
	return carnumber;
}

int afstandeninlezen()
{
	int i=0;
	int j=0;
	int skip=0;
	int returnvalue=0;
	int length=6;
	int bit1[8];
	int bit2[8];
	int timing=0;
	int mssgnr=0;

	for(i=0;i<10;i++)
	{
		afstandmssg[i][0]=2;

	}

	while(timing<5)
	{
		while(1)
		{
			afstandmssg[mssgnr][0]=rs232_port[0];
			if(afstandmssg[mssgnr][0]==69 ||afstandmssg[mssgnr][0]==100)
			{
				timing++;
			}
			else
			{
				break;
			}
		}
		i++;
		while(i<12)
		{
			afstandmssg[mssgnr][i]=rs232_port[0];
			if(afstandmssg[mssgnr][i-1]== 21 && afstandmssg[mssgnr][i]==2) 
			{
				i--;
				i--;
			}
			i++;
		}
		mssgnr++;
		i=0;
	}
	i=0;
	for(j=0;j<mssgnr;j++)
	{
		while(i<length)
		{
			skip=length*2;
			char2bin(afstandmssg[j][skip], bit1);
			char2bin(afstandmssg[j][skip+1], bit2);
			returnvalue = decode(&decodeafstand[j][i], bit1, bit2);
			i++;
		}
	}
	return mssgnr;
}

int extractdistance(int nrmessages, int driven, int pogingnr)
{
	int i=0;
	int succes=0;
	while(i<nrmessages)
	{
		aardevector[decodeafstand[i][3]]=decodeafstand[i][4];
		i++;
	}
	aardevector[driven]=0;
	succes = check(maxID);
	if(succes)
		return 0;
	else
		pogingnr++;

	if(pogingnr>3)
		return -1;
	else
		return pogingnr;
}

void sendaarde()     
{
	int mssgnr=0;
	int codenr=0;
	int skip=0;
	int dle=0;
	int bit1[8];
	int bit2[8];
	int i=0;
	int lengte=0;
	int length=0;
	while(i<maxID)
	{
		newdata[i+2]=aardevector[i];
		i++;
	}
	i=0;
	length=maxID+4;
	while(mssgnr<length)
	{		
		skip=codenr*2;
		encode(data[mssgnr], bit1, bit2);
		codedsend[skip] = bin2char(bit1);
		codedsend[skip+1] = bin2char(bit2);
		if(newdata[mssgnr]==16 && dle==0)
		{
			dle=1;
			mssgnr--;
		}
		else if(dle==1)
		{
			dle=0;
		}
		mssgnr++;
		codenr++;
	}
	lengte=codenr*2;

	//dle start a0 a1 a2...amaxID dle stop =maxID+4
	newdata[0] = 16;//dle
	newdata[1] = 17;//start aarde
	//afstanden (maxID groot)
	newdata[maxID+3] = 16;//dle
	newdata[maxID+4] = 18;//stop aarde

	while(i<lengte)
	{
		rs232_port[0] = codedsend[i];
	}
}

int progressIDs(int inimessages)
{
    int mssg = 1;
    int t = inimessages-1;
	int i=0;
	int j=0;
	int k=0;
	int nieuw=0;
	int ids=1;
	for(i=0;i<8;i++)
	{
		IDs[0][i]=allids[0][i];
	}
	while(mssg<t)
    {
		nieuw=0;
		for(j=0;j<ids;j++)
		{
			for(i = 0; i<8; i++)
			{
				if(allids[mssg][i]==IDs[j][i])
					continue;
				else
				{
					nieuw++;
					break;
				}
			}
			if(nieuw==0)
			{
				break;
			}	
		}
		if(nieuw==ids)
		{
			for(k=0;k<8;k++)
			{
				IDs[ids][k]=allids[mssg][k];
			}
			ids++;
		}
		mssg++;
    }
	return ids;
}

int receiveIDs()
{
    int mssgnr=0;
    int timing= 0;
    int i = 0;
	int j = 0;
	int skip=0;
	int length=12;
	int bit1[8];
	int bit2[8];
	int returnvalue=0;

    for(mssgnr = 0; mssgnr<100; mssgnr++)
    {
		fullids[mssgnr][0] = rs232_port[0];
		if(fullids[mssgnr][0] == ((char) 69))
        {
            timing++;
        }
        if(timing > 42)
        {
            break;
        }
        for(i = 1; i<24; i++)
        {
            fullids[mssgnr][i] = rs232_port[0];
			if(afstandmssg[mssgnr][i-1]== 21 && afstandmssg[mssgnr][i]==2) 
			{
				i--;
				i--;
			}
        }
    }
	for(j=0;j<mssgnr;j++)
	{
		while(i<8)
		{
			skip=length*2;
			char2bin(afstandmssg[j][skip+4], bit1);
			char2bin(afstandmssg[j][skip+5], bit2);
			returnvalue = decode(&allids[j][i], bit1, bit2);
			i++;
		}
	}
	return mssgnr;
}

int inlezen(int length) //length is zonder hamming
{
	int i=0;
	int j=0;
	int skip=0;
	int returnvalue=0;
	int hamming=2*length;
	int bit1[8];
	int bit2[8];
	int timing=0;
	codeddata[0]=2;
	while(1)
	{
		codeddata[0]=rs232_port[0];
		if(codeddata[0]==69)
		{
			timing++;
		}
		else if(codeddata[0]!=69)
		{
			break;
		}
		if(timing>3)
		{
			return 1;
		}
	}
	i++;
	while(i<hamming)
	{
		codeddata[i]=rs232_port[0];
		if(codeddata[i-1]== 21 && codeddata[i]==2) 
		{
			i--;
			i--;
		}
		i++;
	}
	
	i=0;
	while(i<length)
	{
		skip=length*2;
		char2bin(codeddata[skip], bit1);
		char2bin(codeddata[skip+1], bit2);
		returnvalue = decode(&data[i], bit1, bit2);
		i++;
	}
	return 0;
	
}
